#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/action_node.h>
#include <behaviortree_cpp/loggers/groot2_publisher.h>
#include <iostream>
using namespace std;

/*
SyncActionNode:
    同步节点只返回: SUCCESS,FAILURE, SKIPPED状态
*/
class Sync_Node : public BT::SyncActionNode
{
private:
    uint32_t count_;
public:
    Sync_Node(const std::string& name, const BT::NodeConfiguration& config)
        : SyncActionNode(name, config)
    {
        count_ = 0;
    }

    // tick 执行函数
    BT::NodeStatus tick() override
    {
        unsigned int ret = count_ % 5;
        BT::NodeStatus status;
        cout << "sync同步节点++++++: " << count_;

        switch (ret)
        {
        case 0:
            // status = BT::NodeStatus::IDLE; // NG, sync同步节点没有IDLE状态
            status = BT::NodeStatus::SUCCESS;
            break;
        case 1:
            // status = BT::NodeStatus::RUNNING; // NG, sync同步节点没有RUNNING状态
            status = BT::NodeStatus::FAILURE;
            break;
        case 2:
            status = BT::NodeStatus::SUCCESS;
            break;
        case 3:
            status = BT::NodeStatus::FAILURE;
            break;
        case 4:
            status = BT::NodeStatus::SKIPPED;
            break;
        }

        cout << " status:" << BT::toStr(status) << endl;
        count_++;
        return status;
    }

    // 提供 输入/输出 端口
    static BT::PortsList providedPorts()
    {
        return {};
    }
};


int main()
{
    // 行为工厂
    BT::BehaviorTreeFactory factory;
    // 注册自定义节点
    factory.registerNodeType<Sync_Node>("Sync_Node");
    // 定义行为树 xml结构
    string xml_tree = R"(
<root BTCPP_format="3">
    <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <Sync_Node name="Sync_Node"/>
        </Sequence>
    </BehaviorTree>
</root>
)";
    // 从xml 加载行为树
    BT::Tree tree = factory.createTreeFromText(xml_tree);
    BT::Groot2Publisher publisher(tree);
    // 执行行为树
    for (int i = 0; i < 20; i++)
    {
        tree.tickOnce();
        tree.sleep(std::chrono::seconds(1));
    }

    return 0;
}